﻿#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using starLiGHT;

#endregion

#region COPYRIGHT

/*
    Copyright (c) 2008, 2009, 2010 
        Roland Rosenkranz (Glatzemann@email.de)
*/

#endregion

#region LICENSE

/*
    This file is part of starLiGHT.Collision.

    starLiGHT.Collision is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    starLiGHT.Collision is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with starLiGHT.Collision.  If not, see <http://www.gnu.org/licenses/>.

    
    ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.
*/

#endregion

#region Box2D Copyright
/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#endregion

#region Version Stuff
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 1486                  $:  Revision of last commit *
// * $Author:: glatzemann         $:  Author of last commit   *
// * $Date:: 2010-10-08 15:55:47 #$:  Date of last commit     *
// ************************************************************

// **************[ Box2D-Repository Info ]*********************
// Header-File: n/a
// Revision   : n/a
// Change-Date: n/a
//
// Source-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Collision/b2CollideCircle.cpp
// Revision   : r33
// Change-Date: 2010-01-25
//
// Status     : Finished!
#endregion

namespace starLiGHT.Physic.TwoD
{
    public partial class Collision
    {
        internal static void CollideCircles(out Manifold manifold, CircleShape circleA, ref Transform xfA,
	                                                               CircleShape circleB, ref Transform xfB)
        {
#if !OPTIMIZE
            manifold = new Manifold();
#else
            manifold = ManifoldPool.Instance.Allocate();
#endif

	        manifold.pointCount = 0;

#if !OPTIMIZE
            Vector2 pA = Math.Mul(xfA, circleA.m_p);
	        Vector2 pB = Math.Mul(xfB, circleB.m_p);

	        Vector2 d; d.X = pB.X - pA.X; d.Y = pB.Y - pA.Y;
	        float distSqr; Vector2.Dot(ref d, ref d, out distSqr);
#else
            Vector2 pA;
            Vector2 t; t.X = xfA.R.col1.X * circleA.m_p.X + xfA.R.col2.X * circleA.m_p.Y; t.Y = xfA.R.col1.Y * circleA.m_p.X + xfA.R.col2.Y * circleA.m_p.Y;
            pA.X = xfA.position.X + t.X;
            pA.Y = xfA.position.Y + t.Y;

            Vector2 pB;
            Vector2 t2; t2.X = xfB.R.col1.X * circleB.m_p.X + xfB.R.col2.X * circleB.m_p.Y; t2.Y = xfB.R.col1.Y * circleB.m_p.X + xfB.R.col2.Y * circleB.m_p.Y;
            pB.X = xfB.position.X + t2.X;
            pB.Y = xfB.position.Y + t2.Y;

            Vector2 d; d.X = pB.X - pA.X; d.Y = pB.Y - pA.Y;
            float distSqr = d.X * d.X + d.Y * d.Y;  // Vector2.Dot(ref d, ref d, out distSqr);
#endif

            float rA = circleA.m_radius, rB = circleB.m_radius;
	        float radius = rA + rB;
	        if (distSqr > radius * radius)
	        {
		        return;
	        }

	        manifold.type = ManifoldType.e_circles;
	        manifold.localPoint = circleA.m_p;
	        manifold.localNormal = Vector2.Zero;
	        manifold.pointCount = 1;

	        manifold.points[0].localPoint = circleB.m_p;
	        manifold.points[0].id.key = 0;
        }

#if !ZUNE && !WINDOWS_PHONE && !SILVERLIGHT
        internal unsafe static void CollidePolygonAndCircle(out Manifold manifold,
	                                                 PolygonShape polygonA, ref Transform xfA,
	                                                 CircleShape circleB, ref Transform xfB)
        {
#if !OPTIMIZE
            manifold = new Manifold();
#else
            manifold = ManifoldPool.Instance.Allocate();
#endif

	        manifold.pointCount = 0;

	        // Compute circle position in the frame of the polygon.
#if !OPTIMIZE
            Vector2 c = Math.Mul(xfB, circleB.m_p);
	        Vector2 cLocal = Math.MulT(xfA, c);
#else
            Vector2 cLocal;
            Vector2 t; t.X = xfB.R.col1.X * circleB.m_p.X + xfB.R.col2.X * circleB.m_p.Y; t.Y = xfB.R.col1.Y * circleB.m_p.X + xfB.R.col2.Y * circleB.m_p.Y;
            Vector2 c; c.X = xfB.position.X + t.X; c.Y = xfB.position.Y + t.Y;

            t.X = c.X - xfA.position.X;
            t.Y = c.Y - xfA.position.Y;
            cLocal.X = t.X * xfA.R.col1.X + t.Y * xfA.R.col1.Y;
            cLocal.Y = t.X * xfA.R.col2.X + t.Y * xfA.R.col2.Y;
#endif

	        // Find the min separating edge.
	        int normalIndex = 0;
	        float separation = -PhysicSettings.FLT_MAX;
	        float radius = polygonA.m_radius + circleB.m_radius;
	        int vertexCount = polygonA.m_vertexCount;

            Vector2 tDot; //, vi;
            fixed (Vector2* vertices = polygonA.m_vertices)
            {
                fixed (Vector2* normals = polygonA.m_normals)
                {
                    for (Int32 i = 0; i < vertexCount; ++i)
                    {
                        tDot.X = cLocal.X - vertices[i].X; tDot.Y = cLocal.Y - vertices[i].Y;
                        float s = normals[i].X * tDot.X + normals[i].Y * tDot.Y; // Vector2.Dot(normals[i], tDot);

                        if (s > radius)
                        {
                            // Early out.
                            return;
                        }

                        if (s > separation)
                        {
                            separation = s;
                            normalIndex = i;
                        }
                    }

                    // Vertices that subtend the incident face.
                    int vertIndex1 = normalIndex;
                    int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
                    Vector2 v1 = vertices[vertIndex1];
                    Vector2 v2 = vertices[vertIndex2];

                    // If the center is inside the polygon ...
                    if (separation < PhysicSettings.FLT_EPSILON)
                    {
                        manifold.pointCount = 1;
                        manifold.type = ManifoldType.e_faceA;
                        manifold.localNormal = normals[normalIndex];
                        manifold.localPoint = 0.5f * (v1 + v2);
                        manifold.points[0].localPoint = circleB.m_p;
                        manifold.points[0].id.key = 0;
                        return;
                    }

                    // Compute barycentric coordinates
                    Vector2 t1; t1.X = cLocal.X - v1.X; t1.Y = cLocal.Y - v1.Y;
                    Vector2 t2; t2.X = cLocal.X - v2.X; t2.Y = cLocal.Y - v2.Y;
                    Vector2 t3; t3.X = v2.X - v1.X; t3.Y = v2.Y - v1.Y;
                    Vector2 t4; t4.X = v1.X - v2.X; t4.Y = v1.Y - v2.Y;
                    float u1 = t1.X * t3.X + t1.Y * t3.Y;
                    float u2 = t2.X * t4.X + t2.Y * t4.Y;
                    if (u1 <= 0.0f)
                    {
#if !OPTIMIZE
                        if (Vector2.DistanceSquared(cLocal, v1) > radius * radius)
                        {
                            return;
                        }
#else
                        float tf2 = cLocal.X - v1.X;
                        float tf1 = cLocal.Y - v1.Y;
                        float tf3 = (tf2 * tf2) + (tf1 * tf1);
                        if (tf3 > radius * radius)
                        {
                            return;
                        }
#endif

                        manifold.pointCount = 1;
                        manifold.type = ManifoldType.e_faceA;
#if !OPTIMIZE
                        manifold.localNormal = cLocal - v1;
#else
                        manifold.localNormal.X = cLocal.X - v1.X;
                        manifold.localNormal.Y = cLocal.Y - v1.Y;
#endif
                        manifold.localNormal.Normalize();
                        manifold.localPoint = v1;
                        manifold.points[0].localPoint = circleB.m_p;
                        manifold.points[0].id.key = 0;
                    }
                    else if (u2 <= 0.0f)
                    {
#if !OPTIMIZE
                        if (Vector2.DistanceSquared(cLocal, v2) > radius * radius)
                        {
                            return;
                        }
#else
                        float tf2 = cLocal.X - v2.X;
                        float tf1 = cLocal.Y - v2.Y;
                        float tf3 = (tf2 * tf2) + (tf1 * tf1);
                        if (tf3 > radius * radius)
                        {
                            return;
                        }
#endif
                        manifold.pointCount = 1;
                        manifold.type = ManifoldType.e_faceA;
#if !OPTIMIZE
                        manifold.localNormal = cLocal - v2;
#else
                        manifold.localNormal.X = cLocal.X - v2.X;
                        manifold.localNormal.Y = cLocal.Y - v2.Y;
#endif
                        manifold.localNormal.Normalize();
                        manifold.localPoint = v2;
                        manifold.points[0].localPoint = circleB.m_p;
                        manifold.points[0].id.key = 0;
                    }
                    else
                    {
#if !OPTIMIZE
                        Vector2 faceCenter = 0.5f * (v1 + v2);
                        float _separation = Vector2.Dot(cLocal - faceCenter, normals[vertIndex1]);
#else
                        Vector2 faceCenter; faceCenter.X = 0.5f * (v1.X + v2.X); faceCenter.Y = 0.5f * (v1.Y + v2.Y);
                        Vector2 tl1; tl1.X = cLocal.X - faceCenter.X; tl1.Y = cLocal.Y - faceCenter.Y;
                        float _separation = tl1.X * normals[vertIndex1].X + tl1.Y * normals[vertIndex1].Y; // Vector2.Dot(tl1, normals[vertIndex1]);
#endif
                        if (_separation > radius)
                        {
                            return;
                        }

                        manifold.pointCount = 1;
                        manifold.type = ManifoldType.e_faceA;
                        manifold.localNormal = normals[vertIndex1];
#if !OPTIMIZE
                        manifold.localPoint = faceCenter;
#else
                        manifold.localPoint.X = faceCenter.X; manifold.localPoint.Y = faceCenter.Y;
#endif
                        manifold.points[0].localPoint = circleB.m_p;
                        manifold.points[0].id.key = 0;
                    }

                }
            }
        }
#else
        internal static void CollidePolygonAndCircle(out Manifold manifold,
	                                                 PolygonShape polygonA, ref Transform xfA,
	                                                 CircleShape circleB, ref Transform xfB)
        {
            manifold = new Manifold();

	        manifold.pointCount = 0;

	        // Compute circle position in the frame of the polygon.
#if !OPTIMIZE
            Vector2 c = Math.Mul(xfB, circleB.m_p);
	        Vector2 cLocal = Math.MulT(xfA, c);
#else
            Vector2 cLocal;
            Vector2 t; t.X = xfB.R.col1.X * circleB.m_p.X + xfB.R.col2.X * circleB.m_p.Y; t.Y = xfB.R.col1.Y * circleB.m_p.X + xfB.R.col2.Y * circleB.m_p.Y;
            Vector2 c; c.X = xfB.position.X + t.X; c.Y = xfB.position.Y + t.Y;

            t.X = c.X - xfA.position.X;
            t.Y = c.Y - xfA.position.Y;
            cLocal.X = t.X * xfA.R.col1.X + t.Y * xfA.R.col1.Y;
            cLocal.Y = t.X * xfA.R.col2.X + t.Y * xfA.R.col2.Y;
#endif

	        // Find the min separating edge.
	        int normalIndex = 0;
	        float separation = -PhysicSettings.FLT_MAX;
	        float radius = polygonA.m_radius + circleB.m_radius;
	        int vertexCount = polygonA.m_vertexCount;

            Vector2 tDot, vi;
            Vector2[] vertices = polygonA.m_vertices;
            {
                Vector2[] normals = polygonA.m_normals;
                {
                    for (Int32 i = 0; i < vertexCount; ++i)
                    {
                        tDot.X = cLocal.X - vertices[i].X; tDot.Y = cLocal.Y - vertices[i].Y;
                        float s = normals[i].X * tDot.X + normals[i].Y * tDot.Y; // Vector2.Dot(normals[i], tDot);

                        if (s > radius)
                        {
                            // Early out.
                            return;
                        }

                        if (s > separation)
                        {
                            separation = s;
                            normalIndex = i;
                        }
                    }

                    // Vertices that subtend the incident face.
                    int vertIndex1 = normalIndex;
                    int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
                    Vector2 v1 = vertices[vertIndex1];
                    Vector2 v2 = vertices[vertIndex2];

                    // If the center is inside the polygon ...
                    if (separation < PhysicSettings.FLT_EPSILON)
                    {
                        manifold.pointCount = 1;
                        manifold.type = ManifoldType.e_faceA;
                        manifold.localNormal = normals[normalIndex];
                        manifold.localPoint = 0.5f * (v1 + v2);
                        manifold.points[0].localPoint = circleB.m_p;
                        manifold.points[0].id.key = 0;
                        return;
                    }

                    // Compute barycentric coordinates
                    Vector2 t1; t1.X = cLocal.X - v1.X; t1.Y = cLocal.Y - v1.Y;
                    Vector2 t2; t2.X = cLocal.X - v2.X; t2.Y = cLocal.Y - v2.Y;
                    Vector2 t3; t3.X = v2.X - v1.X; t3.Y = v2.Y - v1.Y;
                    Vector2 t4; t4.X = v1.X - v2.X; t4.Y = v1.Y - v2.Y;
                    float u1 = t1.X * t3.X + t1.Y * t3.Y;
                    float u2 = t2.X * t4.X + t2.Y * t4.Y;
                    if (u1 <= 0.0f)
                    {
#if !OPTIMIZE
                        if (Vector2.DistanceSquared(cLocal, v1) > radius * radius)
                        {
                            return;
                        }
#else
                        float tf2 = cLocal.X - v1.X;
                        float tf1 = cLocal.Y - v1.Y;
                        float tf3 = (tf2 * tf2) + (tf1 * tf1);
                        if (tf3 > radius * radius)
                        {
                            return;
                        }
#endif

                        manifold.pointCount = 1;
                        manifold.type = ManifoldType.e_faceA;
#if !OPTIMIZE
                        manifold.localNormal = cLocal - v1;
#else
                        manifold.localNormal.X = cLocal.X - v1.X;
                        manifold.localNormal.Y = cLocal.Y - v1.Y;
#endif
                        manifold.localNormal.Normalize();
                        manifold.localPoint = v1;
                        manifold.points[0].localPoint = circleB.m_p;
                        manifold.points[0].id.key = 0;
                    }
                    else if (u2 <= 0.0f)
                    {
#if !OPTIMIZE
                        if (Vector2.DistanceSquared(cLocal, v2) > radius * radius)
                        {
                            return;
                        }
#else
                        float tf2 = cLocal.X - v2.X;
                        float tf1 = cLocal.Y - v2.Y;
                        float tf3 = (tf2 * tf2) + (tf1 * tf1);
                        if (tf3 > radius * radius)
                        {
                            return;
                        }
#endif
                        manifold.pointCount = 1;
                        manifold.type = ManifoldType.e_faceA;
#if !OPTIMIZE
                        manifold.localNormal = cLocal - v2;
#else
                        manifold.localNormal.X = cLocal.X - v2.X;
                        manifold.localNormal.Y = cLocal.Y - v2.Y;
#endif
                        manifold.localNormal.Normalize();
                        manifold.localPoint = v2;
                        manifold.points[0].localPoint = circleB.m_p;
                        manifold.points[0].id.key = 0;
                    }
                    else
                    {
#if !OPTIMIZE
                        Vector2 faceCenter = 0.5f * (v1 + v2);
                        float _separation = Vector2.Dot(cLocal - faceCenter, normals[vertIndex1]);
#else
                        Vector2 faceCenter; faceCenter.X = 0.5f * (v1.X + v2.X); faceCenter.Y = 0.5f * (v1.Y + v2.Y);
                        Vector2 tl1; tl1.X = cLocal.X - faceCenter.X; tl1.Y = cLocal.Y - faceCenter.Y;
                        float _separation = tl1.X * normals[vertIndex1].X + tl1.Y * normals[vertIndex1].Y; // Vector2.Dot(tl1, normals[vertIndex1]);
#endif
                        if (_separation > radius)
                        {
                            return;
                        }

                        manifold.pointCount = 1;
                        manifold.type = ManifoldType.e_faceA;
                        manifold.localNormal = normals[vertIndex1];
#if !OPTIMIZE
                        manifold.localPoint = faceCenter;
#else
                        manifold.localPoint.X = faceCenter.X; manifold.localPoint.Y = faceCenter.Y;
#endif
                        manifold.points[0].localPoint = circleB.m_p;
                        manifold.points[0].id.key = 0;
                    }

                }
            }
        }
#endif


    }
}
